/*
 * Copyright (c) 2025 STMROS Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "rclcpp/rclcpp.hpp"
#include "stmros/stmros.hpp"
#include <signal.h>
#include <memory>

// 全局节点指针，用于信号处理
std::shared_ptr<rclcpp::Node> g_node;
std::shared_ptr<stmros::StmRos> g_stmros;

// 信号处理函数
void signal_handler(int sig) {
  RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...", sig);
  
  if (g_stmros) {
    g_stmros->shutdown();
  }
  
  rclcpp::shutdown();
  exit(0);
}

int main(int argc, char ** argv) {
  // 初始化ROS
  rclcpp::init(argc, argv);
  
  // 创建多线程执行器
  rclcpp::executors::MultiThreadedExecutor executor;
  
  // 创建节点
  g_node = std::make_shared<rclcpp::Node>("stmros_node");
  
  // 添加节点到执行器
  executor.add_node(g_node);
  
  // 创建STMROS实例
  g_stmros = std::make_shared<stmros::StmRos>(g_node);
  
  // 初始化STMROS
  if (!g_stmros->initialize()) {
    RCLCPP_ERROR(g_node->get_logger(), "Failed to initialize STMROS");
    return 1;
  }
  
  // 设置信号处理
  signal(SIGINT, signal_handler);
  signal(SIGTERM, signal_handler);
  
  RCLCPP_INFO(g_node->get_logger(), "STMROS node started successfully");
  
  // 运行执行器
  executor.spin();
  
  // 清理
  g_stmros->shutdown();
  rclcpp::shutdown();
  
  return 0;
}